{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "eeMrMI0-1Dhu"
   },
   "outputs": [],
   "source": [
    "# Let's do all of our imports here.\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt, mpld3\n",
    "\n",
    "from pydrake.all import RigidTransform, RollPitchYaw, RotationMatrix\n",
    "\n",
    "from manipulation import running_as_notebook\n",
    "\n",
    "if running_as_notebook:\n",
    "    mpld3.enable_notebook()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "QuFmomDOxoAR",
    "tags": []
   },
   "outputs": [],
   "source": [
    "num_points_per_side = 7\n",
    "N = num_points_per_side * 4\n",
    "x = np.arange(-1, 1, 2 / num_points_per_side)\n",
    "half_width = 2\n",
    "half_height = 1\n",
    "top = np.vstack((half_width * x, half_height + 0 * x))\n",
    "right = np.vstack((half_width + 0 * x, -half_height * x))\n",
    "bottom = np.vstack((-half_width * x, -half_height + 0 * x))\n",
    "left = np.vstack((-half_width + 0 * x, half_height * x))\n",
    "p_m = np.vstack(\n",
    "    (\n",
    "        np.hstack((top, right, bottom, left)),\n",
    "        np.zeros((1, 4 * num_points_per_side)),\n",
    "    )\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def min_distance(x, y):\n",
    "    return np.sqrt(np.min((x - p_m[0, :]) ** 2 + (y - p_m[1, :]) ** 2))\n",
    "\n",
    "\n",
    "ax = plt.subplot()\n",
    "ax.plot(p_m[0, :], p_m[1, :], \"b.\")\n",
    "ax.fill(p_m[0, :], p_m[1, :], \"lightblue\", alpha=0.5)\n",
    "\n",
    "X, Y = np.meshgrid(np.linspace(-3, 3, 150), np.linspace(-2, 2, 150))\n",
    "MinDistance = np.vectorize(min_distance)\n",
    "Z = MinDistance(X, Y)\n",
    "\n",
    "CS = ax.contour(X, Y, Z, 3)\n",
    "ax.clabel(CS, inline=True, fontsize=10)\n",
    "ax.axis(\"equal\")\n",
    "ax.axis(\"off\")\n",
    "plt.savefig(\"rectangle_points_distance.svg\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def signed_distance(x, y):\n",
    "    x = np.abs(x)\n",
    "    y = np.abs(y)\n",
    "    if x >= 2.0 and y >= 1.0:\n",
    "        return np.sqrt((x - 2) ** 2 + (y - 1) ** 2)\n",
    "    if y <= 1.0 and x <= 2.0:\n",
    "        return np.max([x - 2, y - 1])\n",
    "    if x > 2.0:\n",
    "        return x - 2\n",
    "    return y - 1\n",
    "\n",
    "\n",
    "ax = plt.subplot()\n",
    "p_m_closed = np.hstack(\n",
    "    (p_m, p_m[:, 0].reshape(3, 1))\n",
    ")  # close the box for this plot\n",
    "ax.plot(p_m_closed[0, :], p_m_closed[1, :], \"b-\")\n",
    "ax.fill(p_m[0, :], p_m[1, :], \"lightblue\", alpha=0.5)\n",
    "\n",
    "X, Y = np.meshgrid(np.linspace(-3, 3, 150), np.linspace(-2, 2, 150))\n",
    "SignedDistance = np.vectorize(signed_distance)\n",
    "Z = SignedDistance(X, Y)\n",
    "\n",
    "CS = ax.contour(X, Y, Z, 6)\n",
    "ax.clabel(CS, inline=True, fontsize=10)\n",
    "ax.axis(\"equal\")\n",
    "ax.axis(\"off\")\n",
    "plt.savefig(\"rectangle_signed_distance.svg\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ax = plt.subplot()\n",
    "# ax.plot(p_m[0, :], p_m[1, :], 'b.')\n",
    "ax.fill(p_m[0, :], p_m[1, :], \"lightsalmon\", alpha=0.5)\n",
    "\n",
    "Nv = 15\n",
    "ax.plot(p_m[0, :Nv], p_m[1, :Nv], \"r.\")\n",
    "c = np.array([3, 2])\n",
    "o = np.hstack((p_m[:2, :Nv], c.reshape((2, 1))))\n",
    "for i in range(Nv):\n",
    "    ax.plot([o[0, i], c[0]], [o[1, i], c[1]], \"r--\", alpha=0.4)\n",
    "ax.fill(o[0, :], o[1, :], \"lightgreen\", alpha=0.5)\n",
    "o_closed = np.hstack((c.reshape(2, 1), o))\n",
    "ax.plot(o_closed[0, :], o_closed[1, :], \"g-\")\n",
    "theta = 0.8\n",
    "R = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]])\n",
    "c_box = c + np.matmul(R, 0.3 * np.array([[0, 1, 1, 0], [-1, -1, 1, 1]])).T\n",
    "ax.fill(c_box[:, 0], c_box[:, 1], \"lightgray\")\n",
    "ax.text(-0.3, -0.1, \"object\", color=\"r\")\n",
    "ax.text(0.5, 1.8, \"free space\", color=\"g\")\n",
    "ax.text(2.15, 2.25, \"camera\")\n",
    "ax.axis(\"equal\")\n",
    "ax.axis(\"off\")\n",
    "plt.savefig(\"free_space.svg\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "name": "Robotic Manipulation - Basic Pick and Place.ipynb",
   "provenance": [],
   "toc_visible": true
  },
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}